﻿using System;
using System.Collections.Generic;
using System.Text;
using RoBOSSCommunicator;
using Geometry;
using System.Globalization;
using RobotRace.environment.objects;
using FIRADriverLibrary;

namespace RobotRace.Geometry
{
    class XmlValuesMapper
    {
        public static Wall2D mapGeomFromXmlToWall2D(Geom geom)
        {
            Wall2D wall = null;
            try
            {
                double x = Double.Parse(geom.GetDefinitionAttribute("position_x"), CultureInfo.InvariantCulture);
                double y = Double.Parse(geom.GetDefinitionAttribute("position_y"), CultureInfo.InvariantCulture);
                double length = Double.Parse(geom.GetDefinitionAttribute("size_x"), CultureInfo.InvariantCulture);
                double rotation = Double.Parse(geom.GetDefinitionAttribute("rotation_z"), CultureInfo.InvariantCulture);
                Point2D rotationCenter = new Point2D(x, y);
                double x1 = x - length / 2;
                double x2 = x + length / 2;

                wall = new Wall2D(rotate(x1, y, rotationCenter, rotation), rotate(x2, y, rotationCenter, rotation));

                Console.WriteLine(wall);
            }
            catch (Exception ex)
            {
                Console.WriteLine("Error while parsing double value");
            }
            return wall;
        }

        private static Point2D rotate(double x, double y, Point2D rotationCenter, double angle)
        {
            x -= rotationCenter.x;
            y -= rotationCenter.y;
            double newX = x * Math.Cos(angle) - y * Math.Sin(angle);
            double newY = x * Math.Sin(angle) + y * Math.Cos(angle);
            return new Point2D(newX + rotationCenter.x, newY+rotationCenter.y);
        }

        public static Point2D mapRobotStartingPositionFromXmlToPoint2D(FIRARobotState robot)
        {
                return new Point2D(robot.x, robot.y);

        }
    }
}
